Skip to content

Fix AGL velocity estimation using squared acceleration weight factor #11255

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom
Open
sensei-hacker wants to merge 2 commits into
base: maintenance-9.x
Choose a base branch
from
Open

Fix AGL velocity estimation using squared acceleration weight factor #11255

sensei-hacker wants to merge 2 commits into from
+2 −2

Conversation

@sensei-hacker
Copy link
Member

@sensei-hacker sensei-hacker commented Jan 17, 2026 •
edited
Loading

User description

Summary

Fixes mathematical inconsistency in AGL velocity estimation where velocity update uses squared acceleration weight factor while altitude update uses linear weight factor.

Problem

In navigation_pos_estimator_agl.c line 152, the velocity update incorrectly uses sq(accWeightFactor) while line 151 (altitude update) correctly uses linear accWeightFactor. This creates physical inconsistency where position and velocity respond differently to the same acceleration measurement.

Impact with accWeightFactor=0.3

  • Altitude receives 30% of acceleration contribution (correct)
  • Velocity receives 9% of acceleration contribution (wrong - squared factor)
  • Result: Large error in velocity response during degraded accelerometer conditions

Changes

  • Removed sq() wrapper from accWeightFactor in AGL velocity update (line 152)
  • Makes velocity integration physically consistent with altitude integration
  • Both now use linear weight factor as per standard kinematic equations

Testing

  • Built SITL target successfully with fix applied
  • Code review performed with inav-code-review agent - no issues found
  • Fix verified mathematically: both altitude and velocity now scale linearly with weight factor
  • Theoretical analysis confirms 50% velocity error is eliminated

Code Review

Reviewed with inav-code-review agent - APPROVED. The fix correctly addresses a mathematical inconsistency and makes the kinematic integration physically consistent.

Related Issues

May fix #10314
May be related to #10567

PR Type

Bug fix

Description

  • Removes squared acceleration weight factor from AGL velocity update

  • Makes velocity integration consistent with altitude integration

  • Fixes velocity error during degraded accelerometer conditions

  • Both altitude and velocity now use linear weight factor scaling

Diagram Walkthrough

flowchart LR
  accel["Acceleration measurement"]
  weight["Weight factor"]
  alt["Altitude update"]
  vel["Velocity update"]
  accel -- "linear factor" --> alt
  accel -- "squared factor (BEFORE)" --> vel
  accel -- "linear factor (AFTER)" --> vel
  weight --> alt
  weight --> vel
Loading

File Walkthrough
Relevant files
Bug fix
navigation_pos_estimator_agl.c
Remove squared weight factor from velocity update               

src/main/navigation/navigation_pos_estimator_agl.c

  • Removed sq() wrapper from accWeightFactor in AGL velocity update at
    line 152
  • Changed velocity integration to use linear weight factor instead of
    squared
  • Aligns velocity update with altitude update which already uses linear
    weight factor
  • Maintains kinematic consistency between position and velocity
    integration
 +1/-1     

@sensei-hacker
Fix AGL velocity estimation using squared acceleration weight factor
798a612 
The AGL velocity update was incorrectly using squared acceleration
weight factor (sq(accWeightFactor)) while the altitude update uses
linear weight. This creates physical inconsistency where position
and velocity respond differently to the same acceleration.

With accWeightFactor=0.5 (moderate confidence):
- Altitude receives 50% of acceleration (correct)
- Velocity was receiving 25% of acceleration (wrong - squared)

This fix removes sq() from the velocity update to match the altitude
update, making both integrations physically consistent with standard
kinematic equations.

May fix iNavFlight#10314
May be related to iNavFlight#10567
@sensei-hacker sensei-hacker added this to the 9.1 milestone Jan 17, 2026
@qodo-code-review
Copy link
Contributor

qodo-code-review bot commented Jan 17, 2026 •
edited
Loading

PR Compliance Guide 🔍

All compliance sections have been disabled in the configurations.

qodo-code-review[bot]
qodo-code-review bot reviewed Jan 17, 2026
View reviewed changes
@sensei-hacker
Fix comment/code mismatch in position estimator weight factors
5cf56e8 
Comments incorrectly stated 0.3 when code uses 0.5f.
Updated comments to match actual implementation.
@sensei-hacker sensei-hacker mentioned this pull request Jan 22, 2026
Open
@Jetrell Jetrell mentioned this pull request Feb 3, 2026
Merged
@Jetrell
Copy link

Jetrell commented Feb 5, 2026

Tested with Matek PMW3901. And I didn't find the results conclusive. Maybe because this sensor has such limited range under intense UV from the sun.
I also left a comment here.

I think it requires testing with a better lidar sensor like the MTF-01, which I don't have. To properly confirm the fix works.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@qodo-code-review qodo-code-review[bot] qodo-code-review[bot] left review comments

Assignees

No one assigned

Labels

Review effort 2/5

Projects

None yet

Milestone

9.1

Development

Successfully merging this pull request may close these issues.

2 participants

@sensei-hacker @Jetrell